fix(simple_planning_simulator): fix steering bias model #6240

TakaHoribe · 2024-01-30T16:01:33Z

Description

In the old implementation, steering_bias was just added in the measurement, thus the behavior was "the vehicle yaw dynamics totally follow the steering command, but the steering output seems different". However, the expected behavior is "the steering output seems to be ideal, but the vehicle yaw dynamics does not match its steering angle". This PR fixes the behavior.

Tests performed

Add steer_bias param in yaml. (e.g. set to 0.01)
Set steering_offset.enable_auto_steering_offset_removal to false in mpc.param.yaml
Run psim
See the vehicle trajectory following is slightly biased to the right/left.

You can also check the steering offset topic /control/trajectory_follower/controller_node_exe/output/estimated_steer_offset converges to the bias value if you set the offset_removal true.

Notes for reviewers

None

Interface changes

None

Effects on system behavior

None by default.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

I've confirmed the contribution guidelines.
The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

The PR follows the pull request guidelines.
The PR has been properly tested.
The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

There are no open discussions or they are tracked via tickets.
The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

Signed-off-by: Takamasa Horibe <[email protected]>

tkimura4

LGTM

Signed-off-by: Takamasa Horibe <[email protected]>

codecov · 2024-01-31T00:01:56Z

Codecov Report

Attention: 6 lines in your changes are missing coverage. Please review.

Comparison is base (0d10e81) 14.91% compared to head (8101055) 14.45%.
Report is 7 commits behind head on main.

Files	Patch %	Lines
...cle_model/sim_model_delay_steer_map_acc_geared.cpp	0.00%	4 Missing ⚠️
...nning_simulator/simple_planning_simulator_core.cpp	0.00%	0 Missing and 1 partial ⚠️
...ulator/vehicle_model/sim_model_delay_steer_vel.cpp	80.00%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #6240      +/-   ##
==========================================
- Coverage   14.91%   14.45%   -0.47%     
==========================================
  Files        1817     1906      +89     
  Lines      125357   129364    +4007     
  Branches    37640    37363     -277     
==========================================
- Hits        18697    18694       -3     
- Misses      85651    89779    +4128     
+ Partials    21009    20891     -118

Flag	Coverage Δ		*Carryforward flag
differential	`47.23% <66.66%> (?)`
total	`14.43% <ø> (-0.48%)`	⬇️	Carriedforward from c625256

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

kosuke55 · 2024-01-31T06:34:58Z

...planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp

@@ -127,7 +128,7 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel(
  Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_);
  d_state(IDX::X) = vel * cos(yaw);
  d_state(IDX::Y) = vel * sin(yaw);
-  d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_;
+  d_state(IDX::YAW) = vel * std::tan(steer + steer_bias_) / wheelbase_;


Only yaw among the states has a biased value. each of the states should be unified as to whether it is an actual value or a sensor value.

Signed-off-by: kosuke55 <[email protected]>

kosuke55 · 2024-01-31T06:39:11Z

Only yaw among the states has a biased value. each of the states should be unified as to whether it is an actual value or a sensor value.

fixed in e4dadc5

All states are actual values, and the values obtained by getSteer() are biased and used in the calculation of the steer_rate in calcModel() and published as an output report.

kosuke55 · 2024-01-31T06:39:37Z

steer_bias-2024-01-31_15.14.41.mp4

Signed-off-by: kosuke55 <[email protected]>

kosuke55 · 2024-01-31T06:51:22Z

@TakaHoribe could you please check my commits? 🙏

TakaHoribe · 2024-02-05T03:35:39Z

...g_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp

@@ -67,11 +68,12 @@ double SimModelDelaySteerAccGeared::getAx()
 }
 double SimModelDelaySteerAccGeared::getWz()
 {
-  return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_;
+  return state_(IDX::VX) * std::tan(state_(IDX::STEER) + steer_bias_) / wheelbase_;


@kosuke55 You can remove this change.

fixed
c625256

Signed-off-by: kosuke55 <[email protected]>

kosuke55 · 2024-02-05T04:15:54Z

@TakaHoribe

after c625256
works well!

Add steer_bias param in yaml. (e.g. set to 0.01)
Set steering_offset.enable_auto_steering_offset_removal to false in mpc.param.yaml
Run psim
See the vehicle trajectory following is slightly biased to the right/left.

steer_bias_remove_off-2024-02-05_13.12.08.mp4

You can also check the steering offset topic /control/trajectory_follower/controller_node_exe/output/estimated_steer_offset converges to the bias value if you set the offset_removal true.

steer_bias-2024-02-05_13.08.49.mp4

steer_bias-2024-02-05_13.09.46.mp4

...mulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp

...planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp

...planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp

…tor/vehicle_model/sim_model_delay_steer_acc.cpp

…tor/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp

…tor/vehicle_model/sim_model_delay_steer_vel.cpp

…dation#6240) * fix(simple_planning_simulator): fix steering bias model Signed-off-by: Takamasa Horibe <[email protected]> * remove old implementation Signed-off-by: Takamasa Horibe <[email protected]> * fix initialize order Signed-off-by: Takamasa Horibe <[email protected]> * fix yawrate measurement Signed-off-by: Takamasa Horibe <[email protected]> * remove unused code Signed-off-by: Takamasa Horibe <[email protected]> * add bias to steer rate Signed-off-by: kosuke55 <[email protected]> * add comments Signed-off-by: kosuke55 <[email protected]> * fix getWz() Signed-off-by: kosuke55 <[email protected]> * Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp * Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp * Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp --------- Signed-off-by: Takamasa Horibe <[email protected]> Signed-off-by: kosuke55 <[email protected]> Co-authored-by: kosuke55 <[email protected]>

TakaHoribe added 3 commits January 31, 2024 00:31

fix(simple_planning_simulator): fix steering bias model

d51e693

Signed-off-by: Takamasa Horibe <[email protected]>

remove old implementation

d1fbfc7

Signed-off-by: Takamasa Horibe <[email protected]>

fix initialize order

aed992d

Signed-off-by: Takamasa Horibe <[email protected]>

TakaHoribe requested a review from tkimura4 as a code owner January 30, 2024 16:01

TakaHoribe requested a review from kosuke55 January 30, 2024 16:01

TakaHoribe added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Jan 30, 2024

github-actions bot added type:documentation Creating or refining documentation. (auto-assigned) component:simulation Virtual environment setups and simulations. (auto-assigned) labels Jan 30, 2024

fix yawrate measurement

cf188f2

Signed-off-by: Takamasa Horibe <[email protected]>

tkimura4 approved these changes Jan 30, 2024

View reviewed changes

remove unused code

6018a2c

Signed-off-by: Takamasa Horibe <[email protected]>

TakaHoribe enabled auto-merge (squash) January 30, 2024 16:25

TakaHoribe disabled auto-merge January 30, 2024 23:29

kosuke55 reviewed Jan 31, 2024

View reviewed changes

add bias to steer rate

e4dadc5

Signed-off-by: kosuke55 <[email protected]>

add comments

30ea518

Signed-off-by: kosuke55 <[email protected]>

TakaHoribe commented Feb 5, 2024

View reviewed changes

kosuke55 added 2 commits February 5, 2024 12:58

Merge remote-tracking branch 'awf/main' into fix-steer-bias

3eeadef

fix getWz()

c625256

Signed-off-by: kosuke55 <[email protected]>

TakaHoribe commented Feb 5, 2024

View reviewed changes

...mulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp Outdated Show resolved Hide resolved

TakaHoribe commented Feb 5, 2024

View reviewed changes

...planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp Outdated Show resolved Hide resolved

TakaHoribe commented Feb 5, 2024

View reviewed changes

...planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp Outdated Show resolved Hide resolved

TakaHoribe added 3 commits February 5, 2024 23:51

Update simulator/simple_planning_simulator/src/simple_planning_simula…

a0259fc

…tor/vehicle_model/sim_model_delay_steer_acc.cpp

Update simulator/simple_planning_simulator/src/simple_planning_simula…

b3e9f71

…tor/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp

Update simulator/simple_planning_simulator/src/simple_planning_simula…

8101055

…tor/vehicle_model/sim_model_delay_steer_vel.cpp

TakaHoribe enabled auto-merge (squash) February 5, 2024 14:52

TakaHoribe merged commit 0b90a97 into autowarefoundation:main Feb 5, 2024
21 of 23 checks passed

TakaHoribe deleted the fix-steer-bias branch February 5, 2024 15:17

tier4-autoware-public-bot bot mentioned this pull request Feb 7, 2024

chore: sync upstream tier4/autoware.universe#1130

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(simple_planning_simulator): fix steering bias model #6240

fix(simple_planning_simulator): fix steering bias model #6240

TakaHoribe commented Jan 30, 2024 •

edited

Loading

tkimura4 left a comment

codecov bot commented Jan 31, 2024 •

edited

Loading

kosuke55 Jan 31, 2024

kosuke55 commented Jan 31, 2024

kosuke55 commented Jan 31, 2024

kosuke55 commented Jan 31, 2024

TakaHoribe Feb 5, 2024

kosuke55 Feb 5, 2024

kosuke55 commented Feb 5, 2024 •

edited

Loading

fix(simple_planning_simulator): fix steering bias model #6240

fix(simple_planning_simulator): fix steering bias model #6240

Conversation

TakaHoribe commented Jan 30, 2024 • edited Loading

Description

Related links

Tests performed

Notes for reviewers

Interface changes

Effects on system behavior

Pre-review checklist for the PR author

In-review checklist for the PR reviewers

Post-review checklist for the PR author

tkimura4 left a comment

Choose a reason for hiding this comment

codecov bot commented Jan 31, 2024 • edited Loading

Codecov Report

kosuke55 Jan 31, 2024

Choose a reason for hiding this comment

kosuke55 commented Jan 31, 2024

kosuke55 commented Jan 31, 2024

kosuke55 commented Jan 31, 2024

TakaHoribe Feb 5, 2024

Choose a reason for hiding this comment

kosuke55 Feb 5, 2024

Choose a reason for hiding this comment

kosuke55 commented Feb 5, 2024 • edited Loading

TakaHoribe commented Jan 30, 2024 •

edited

Loading

codecov bot commented Jan 31, 2024 •

edited

Loading

kosuke55 commented Feb 5, 2024 •

edited

Loading